/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : rc.cpp
  * Description        :
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : 2020年10月23日
  ******************************************************************************
  */
/* USER CODE END Header */

#include "rc.hpp"
#include "ringbuffer.hpp"
#include "rc_rtcm_process.hpp"

uint8_t rtcm_cnt=0;
uint8_t last_rtcm_cnt=0;
uint8_t rtcm_data[1000];

RC rc(UART4,(char *)"rc");

void UART4_IRQHandler(void)
{
	if(LL_USART_IsActiveFlag_IDLE(UART4))
	{
		LL_USART_ClearFlag_IDLE(UART4);
		LL_DMA_DisableStream(DMA1,LL_DMA_STREAM_2);
		LL_DMA_ClearFlag_DME2(DMA1);
		LL_DMA_ClearFlag_HT2(DMA1);
		LL_DMA_ClearFlag_TC2(DMA1);
		LL_DMA_ClearFlag_TE2(DMA1);
		LL_DMA_ClearFlag_FE2(DMA1);

		LL_USART_ClearFlag_CM(UART4);
		LL_USART_ClearFlag_EOB(UART4);
		LL_USART_ClearFlag_FE(UART4);
		LL_USART_ClearFlag_LBD(UART4);
		LL_USART_ClearFlag_NE(UART4);
		LL_USART_ClearFlag_ORE(UART4);
		LL_USART_ClearFlag_PE(UART4);
		LL_USART_ClearFlag_RTO(UART4);
		LL_USART_ClearFlag_TC(UART4);
		LL_USART_ClearFlag_WKUP(UART4);
		LL_USART_ClearFlag_nCTS(UART4);
		LL_USART_ClearFlag_IDLE(UART4);
		rc.RxDataSize = RC_RX_LEN - LL_DMA_GetDataLength(DMA1, LL_DMA_STREAM_2);

		do{
//			if((uint8_t)rc.RxRawDat[27]==0xFD)
//			{
//				memcpy(&rtcm_data[rtcm_cnt*30],(uint8_t *)&rc.RxRawDat[30],30);
//				++rtcm_cnt;
//			}
//			else
//			{
//				rtcm_cnt=0;
//			}
//			if(last_rtcm_cnt!=0 && rtcm_cnt==0)
//			{
//				uart6_Send_DMA(rtcm_data,last_rtcm_cnt*30);
//			}
//
//			last_rtcm_cnt = rtcm_cnt;
//
//			if(rc.RxRawDat[0]!='$' || rc.RxRawDat[26]!='\n')break;
//			if(rc.LockRx == HAL_LOCKED)break;
//			rc.LockRx = HAL_LOCKED;
//			memcpy(rc.RxDat,rc.RxRawDat,rc.RxDataSize);
//			rc.RxDat[rc.RxDataSize]=0;
//			rc.LockRx = HAL_UNLOCKED;
//			rc.RxFlag = true;   //收到完整一帧
//

			Write_to_ringbuffer((uint8_t *)rc.RxRawDat, rc.RxDataSize);

//			BaseType_t YieldRequired = xTaskResumeFromISR(rcTaskHandle);
//			if(YieldRequired==pdTRUE)
//			{
//				/*如果函数xTaskResumeFromISR()返回值为pdTRUE，那么说明要恢复的这个
//				任务的任务优先级等于或者高于正在运行的任务(被中断打断的任务),所以在
//				退出中断的时候一定要进行上下文切换！*/
//				portYIELD_FROM_ISR(YieldRequired);
//			}
		}while(0);

		LL_DMA_SetDataLength(DMA1, LL_DMA_STREAM_2, RC_RX_LEN);
		LL_DMA_EnableStream(DMA1, LL_DMA_STREAM_2);
	}
}
void DMA1_Stream2_IRQHandler(void)
{

}

void RC::rc_Init(void)
{
	LockRx = HAL_UNLOCKED;
	RxFlag = false;
	RxDataSize = 0;
	executionTime_us = 0;
	Sta = STA_INI;
	Err = ERR_NONE;

	HIG_THR = 1912;    //油门高位点
	MID_THR = 1420;    //油门中位点
	LOW_THR = 1088;    //油门低位点

	Key[0] = 0;     //4个开关
	Key[1] = 0;     //4个开关
	Key[2] = 1;     //4个开关
	Key[3] = 0;     //4个开关


	ModeChk.CNT = 0;
	ModeChk.CCR = 6;

	dAng = 0.0f;
	Thr = 0.0f;
	for(uint8_t i=0;i<4;i++)
		Val[i] = 0.0f;
	for(uint8_t i=0;i<2;i++)
		Ang[i] = 0.0f;

	float fa,fb,fc;
	fa = LOW_THR/1000.0f;
	fb = MID_THR/1000.0f;
	fc = HIG_THR/1000.0f;
	a = -(4*(5*fc - 9*fa + 5*fa*fb - 5*fb*fc + 6))/(5*(2*fa - 3)*(3*fa - 3*fc - 2*fa*fc + 2*fc*fc));
	b = (4*(5*fa*fa*fb - 5*fb*fc*fc - 9*fa*fa + 5*fc*fc + 9))/(5*(2*fa - 3)*(3*fa - 3*fc - 2*fa*fc + 2*fc*fc));
	c = -(20*fb*fa*fa*fc - 54*fa*fa - 20*fb*fa*fc*fc + 81*fa + 30*fc*fc - 45*fc)/(5*(2*fa - 3)*(3*fa - 3*fc - 2*fa*fc + 2*fc*fc));


	/* 配置接收DMA */
	LL_DMA_DisableStream(DMA1,LL_DMA_STREAM_2);
	LL_DMA_SetPeriphAddress(DMA1, LL_DMA_STREAM_2, (uint32_t)&huart->RDR);
	LL_DMA_SetMemoryAddress(DMA1, LL_DMA_STREAM_2, (uint32_t)RxRawDat);
	LL_DMA_SetDataLength(DMA1, LL_DMA_STREAM_2, RC_RX_LEN);
	LL_DMA_EnableStream(DMA1, LL_DMA_STREAM_2);
	/* 配置接收DMA */

	LL_DMA_ClearFlag_DME2(DMA1);
	LL_DMA_ClearFlag_HT2(DMA1);
	LL_DMA_ClearFlag_TC2(DMA1);
	LL_DMA_ClearFlag_TE2(DMA1);
	LL_DMA_ClearFlag_FE2(DMA1);

	LL_USART_ClearFlag_CM(huart);
	LL_USART_ClearFlag_EOB(huart);
	LL_USART_ClearFlag_FE(huart);
	LL_USART_ClearFlag_LBD(huart);
	LL_USART_ClearFlag_NE(huart);
	LL_USART_ClearFlag_ORE(huart);
	LL_USART_ClearFlag_PE(huart);
	LL_USART_ClearFlag_RTO(huart);
	LL_USART_ClearFlag_TC(huart);
	LL_USART_ClearFlag_WKUP(huart);
	LL_USART_ClearFlag_nCTS(huart);
	LL_USART_ClearFlag_IDLE(huart);

	LL_USART_EnableDMAReq_RX(huart);
	LL_USART_ClearFlag_IDLE(huart);
	LL_USART_EnableIT_IDLE(huart);
	Sta = STA_RUN;
	osDelay(100);

}


void RC::ControlRC_Check(void)
{
	static u16 new_index=0,old_index=1;
	rc_buffer[new_index] = RxFlag;
	rc_check = rc_check + rc_buffer[new_index] - rc_buffer[old_index];
	new_index = (new_index + 1)%RC_CHECK_NUM;
	old_index = (old_index + 1)%RC_CHECK_NUM;
	if(rc_check != 0)
	{
		rc_status=NORMAL;
		ever_rc = 1;
	}
	else if(ever_rc == 0 || rcCommand.Val[3] <= 1200)
	{
		rc_status=CLOSE;
	}
	else
	{
		rc_status=LOST;
	}

	rc_status_msg.rc_status = rc_status;
	rc_status_msg.ever_rc= ever_rc;
	RxFlag = false;
	xQueueOverwrite(queueRC_Status,&rc_status_msg);
}

void RC::rc_Update(void)
{

	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;
	xQueuePeek(queueRCCommand, &rcCommand, 0);

//	if(RxFlag == false) return;	//未更新
//
//	RxFlag = false;

	if(LockRx == HAL_LOCKED)return;
	LockRx = HAL_LOCKED;
	do{
		if(RxDat[0]=='$' && RxDat[PPM_NUM-2]=='\r' && RxDat[PPM_NUM-1]=='\n')
		{
			char CHN[4]="000";
			char *pStr;
			u16 tmp[PWM_RC_NUM];
			for(u8 i=0;i<PWM_RC_NUM;i++)
			{
				strncpy(CHN,RxDat+i*3+1,3);
				tmp[i]=strtol(CHN,&pStr,16) + 420;   //十六进制转换 +420偏置
			}
			//遥控器行为定义 油门2
			static u8 preFlag = 0xFF;
			u8 Chn_Flag_L = 0x00,Chn_Flag_H = 0x00;
			if(tmp[0] < RC_FUN_MIN)Chn_Flag_L|=0x01;
			else if(tmp[0] > RC_FUN_MAX)Chn_Flag_H|=0x01;
			if(tmp[1] < RC_FUN_MIN)Chn_Flag_L|=0x02;
			else if(tmp[1] > RC_FUN_MAX)Chn_Flag_H|=0x02;
			if(tmp[2] < RC_FUN_MIN)Chn_Flag_L|=0x04;
			else if(tmp[2] > RC_FUN_MAX)Chn_Flag_H|=0x04;
			if(tmp[3] < RC_FUN_MIN)Chn_Flag_L|=0x08;
			else if(tmp[3] > RC_FUN_MAX)Chn_Flag_H|=0x08;
			if((Chn_Flag_L|Chn_Flag_H)==0x0F)  //行为持续一定时间视为有效
			{
				if(preFlag == Chn_Flag_H)
				{
					if(++ModeChk.CNT>ModeChk.CCR)
						Mode = (eRC_MODE)Chn_Flag_H;
				}
				else
				{
					preFlag = Chn_Flag_H;
					ModeChk.CNT = 0;
				}
			}
			else preFlag = 0xFF;

			for(u8 i=0;i<PWM_RC_NUM;i++)
			{
				PPM[i] = tmp[i];
				rc_ppm.PPM[i] = PPM[i];
			}

			/*
			 * PPM偏置
			 */
			PPM[0] += 115;
			PPM[1] += 80;
			PPM[3] += 4;
			PPM[5] += -182;

			for(u8 i=0;i<PWM_RC_NUM;i++)
			{
				rcCommand.PPM[i] = PPM[i];
			}

			rcCommand.Val[0] = RC_PPM_MID-PPM[0]; //roll
			rcCommand.Val[1] = RC_PPM_MID-PPM[1]; //pitch
			rcCommand.Val[2] = -(RC_PPM_MID-PPM[3]); //yaw_rate
			rcCommand.Val[3] = PPM[2] - RC_PPM_MID; //thr
			//角度设定
			rcCommand.Ang[0] = rcCommand.Val[0]*0.1f;   //-50°到50°，roll
			rcCommand.Ang[1] = rcCommand.Val[1]*0.1f;   //-50°到50°，pitch
			rcCommand.dAng   = rcCommand.Val[2]*0.5f;   //-250°/s到250°/s，yaw角速度
			rcCommand.Thr    = a*SQR(rcCommand.Val[3])/1000.0f + b*rcCommand.Val[3] + c*1000.0f;//油门换算
			//位置设定
			rcCommand.Uvw[0] = rcCommand.Val[0]*0.0008f; //40m/s
			rcCommand.Uvw[1] = rcCommand.Val[1]*0.0008f; //40m/s
			rcCommand.Uvw[2] = rcCommand.Val[3]*0.0008f; //40m/s

			//遥控器控制开关
			rcCommand.Key[0] = PPM[4]<RC_FUN_MIN?0:(PPM[4]>RC_FUN_MAX?2:1);
			rcCommand.Key[1] = PPM[5]<RC_FUN_MIN?0:(PPM[5]>RC_FUN_MAX?2:1);
			rcCommand.Key[2] = PPM[6]<RC_FUN_MIN?0:(PPM[6]>RC_FUN_MAX?2:1);
			rcCommand.Key[3] = PPM[7]<RC_FUN_MIN?0:(PPM[7]>RC_FUN_MAX?2:1);

			rcCommand.Mode = Mode;
			Update = true;

			xQueueOverwrite(queueRCData,&rc_ppm);
			xQueueOverwrite(queueRCCommand,&rcCommand);
		}
	}while(0);
	LockRx = HAL_UNLOCKED;
	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;
}

//extern "C" void rc_main(void *argument)
//{
//	rc.rc_Init();
//	osDelay(14);
//	for(;;)
//	{
//		vTaskSuspend(rcTaskHandle);
//	}
//}

extern "C" void RC_Check_main(void *argument)
{
	rc.rc_Init();
	osDelay(14);
	for(;;)
	{
		osSemaphoreAcquire(semRcCheck,0xffffffff);
		Rc_And_Rtcm_Task();
		rc.rc_Update();
		rc.ControlRC_Check();		//
	}
}

/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
